/* 
 * University of Plymouth Bioloid Robot Controller
 * for Atmel Mega 128
 * FIRA England Robot Football Team
 * - Based on Dynamixel Evaluation Example (Original Author : Byoung Soo Kim)
 * - Innitial support for executing Motion Editor Pages
 * Author : Joerg Wolf, joerg.wolf -xaxtx- plymouth.ac.uk
 * 
 * Version 0.111  Added numposes to data passed from Alexs code
 */
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <string.h>
#include <stdio.h>

#include "motion.h"
#include "robot.h"
#include "comms.h"
//#include "BLC-Protocol.h"
#include "led.h"
#include "PC2CM5.h"
#include "BioLoidBiped.h"
#include "AX-12.h"
#include "IMU.h"

extern volatile unsigned char TxBuff[MAXBUF_TX], RxBuff[MAXBUF_RX],Packet[PACKET_LEN + 1];       // Transmit and Receive buffers
extern volatile unsigned char *RxPtr, *TxPtr ;  					 // Pointers to buffer positions
extern volatile unsigned int TxBytes; 
extern volatile unsigned int ROBOTSTATUS;
extern volatile int ServoTrim[SERVO_MEMORY];
 
extern uint16_t doPose(int page,char pose);
extern uint16_t doPage(int page);

extern int Stride_Length;
extern int Stride_Height;
extern int Swing;
extern int Stride_X_offset;
extern int Stride_Y_offset;
extern int Ticks_Per_Pose;
extern int Tilt;
extern int Turn;
extern int NumPoses;
extern int Camber;
extern int SideStepTrip;
extern int servo_report;
extern int Foot_Report_Flag;
extern int IMU_Report_Flag;
extern int IMU_ID;
extern int Foot_ID;
extern int UsingPoseSystem;

int forward_counter = 0;
int raw_tilt = 50;
// ----------- extern functions from led.c ---------------
extern void LED_Init(void);
extern void Set_LED(unsigned char LEDno, unsigned char state);

void NextTx(void);
void StoreRxd(unsigned char RxC);
char CheckProtocol(void);					// Check the message for correctness
void OnGoodPacket(void);					// Process message
void OnErr(int ErrNo);
void TxDString(unsigned char  *bData);
void CommsSend(unsigned char *Data, int TxLen, unsigned char Block);
void CommsInitialize(unsigned char bPort, unsigned char bBaudrate, unsigned char bInterrupt);
void CommsSend2Bytes(int l,int r);
void CommsSendInt(int Int );
int UnsignedCharToChar(unsigned char uchar);
//uint16_t crc_compute(uint8_t *buf, uint16_t size);
void SendPacketS(char Command, char *Data); // Data must contain 12 characters.  NULL is optional
void SendPacketI(char Command, int i1, int i2, int i3);

unsigned char OmniID1, OmniID2, OmniID3;	//	IDs of servos used for omni-directional buggy
IMU_RAW Raw;
void CommsSend2Bytes(int l,int r)
{

	unsigned char Data[4];
	Data[0]='[';
	Data[1]=(unsigned char)l;
	Data[2]=(unsigned char)r;
	Data[3]=']';
	CommsSend(Data,4,NO_BLOCK);
}


void CommsSendInt(int Int )
{
	uint16_t lowerbyte,higherbyte;
	unsigned char Data[4];
	lowerbyte = Int & 0x00FF;
	higherbyte = Int;
	higherbyte = higherbyte >> 8;
	Data[0]='[';
	Data[1]=(unsigned char)higherbyte;
	Data[2]=(unsigned char)lowerbyte;
	Data[3]=']';
	CommsSend(Data,4,NO_BLOCK);
}

/************************************************************************\
*   Routine to send debugging information back to the host
* We need to be able to specify whether this routine should or should not
* block
\************************************************************************/
void CommsSend(unsigned char *Data, int TxLen, unsigned char Block)
{
    
	if(TxLen>MAXBUF_TX){ return;} // reject huge packets
	int CurrentEndofMessage = (TxPtr-TxBuff + TxBytes);
	int SpaceLeft = MAXBUF_TX - CurrentEndofMessage;
	switch (Block)
    {
        case NO_BLOCK:
            if (TxBytes>0){return;}
            TxPtr=TxBuff;
            break;
        case MRG_BLOCK:
			//while(TxBytes+TxLen > MAXBUF_TX); //wait until there is enough space in the buffer
            //if (TxBytes+TxLen > MAXBUF_TX){ return; }// reject if packet doesn't fit into buffer
            if(SpaceLeft < TxLen){return;}
			break;
        case FUL_BLOCK:
        default:
            while (TxBytes > 0);                    // Wait for the transmit buffer to clear -- good idea?
            if (TxLen > MAXBUF_TX) TxLen = MAXBUF_TX;             // Try to avoid overflows
            TxPtr=TxBuff;
            break;
    }

    // Copy characters into Transmit buffer
	
    memcpy((void *)TxPtr+TxBytes, Data, TxLen);
    cli();
	TxBytes += TxLen;                           // Set length of buffer to transmit
	sei();
    if (TxBytes > 0)  // Make sure we have something to send
    {
        cli();
			Set_LED(LED_TXD, LED_ON);
			UDR1 = *TxPtr;	// Send character to UART
            TxBytes--;
            TxPtr++;	
        sei();
    }
}

char CheckProtocol(void)
{   //TxDString((unsigned char *)Packet);
	// Check the message for correctness
	/*if(Packet[0]!='['){return -1;}
	if(Packet[MAXBUF_RX-1]!=']'){return -1;}
	uint16_t lowerbyte = Packet[MAXBUF_RX-1-1];
	uint16_t higherbyte = Packet[MAXBUF_RX-1-2];
	uint16_t rec_crc = (higherbyte << 8) + lowerbyte;
	uint16_t crc = crc_compute((uint8_t*)&Packet[1], MAXBUF_RX-4); // packet is 40 bytes including brackets. without brackets and checksum the length is MAX_BUF_RX-4
	if(crc!=rec_crc){return -1;}
	
	return 0;*/
	//Assume All packets are good!
	return 1; //CheckPacket((char *)Packet);
}



/************************************************************************\
*   Routine to process the received good data
\************************************************************************/

void OnGoodPacket()
{
	int	i1 = 0, i2 = 0, i3 = 0;
	extern int IMUReporting, ArmReporting;
	extern volatile int ServoPos[];
	unsigned char temp_char[10];
	int				RetVal = 0;
	int temp_one = 0;
	SplitPacket2((char *)Packet, &i1, &i2, &i3);	//	Not required by some packet types, but shouldn't do any harm
	
	switch (Packet[1])
	{
	case STOP_NOW:
		ROBOTSTATUS = STOP_PAGE;
		break;
		
	case STOP_PAGE:
	case STOP_POSE:
		ROBOTSTATUS = DO_PAGE;
		doPage(16);
		break;
	
	case DO_PAGE:
		ROBOTSTATUS = DO_PAGE;
		doPage(i1);
		break;

	case DO_POSE:
		ROBOTSTATUS = DO_POSE;
		doPose(i1, i2);
		break;
	
	case GET_VERSION:
		SendPacketS(VERSION, "RemCon v14  ");	//	Care!  Text must be 12 characters + NULL
		break;
		
	case OMNI_MODE:		//  Indicates that robot is to use an omni-wheel architecture. Parameters set ServoIDs of wheels
		OmniID1 = (unsigned char)i1;
		OmniID2 = (unsigned char)i2;
		OmniID3 = (unsigned char)i3;

		AX12SetMode(OmniID1, 1);	//	Set free rotating mode for given servo
		AX12SetMode(OmniID2, 1);	//	Set free rotating mode for given servo
		AX12SetMode(OmniID3, 1);	//	Set free rotating mode for given servo
		break;
		
	case OMNI_MOVE:		//  Move omni-direction robot. Parameters set corresponding wheel rotations
		AX12SetFreeSpinSpeed(OmniID1, (short)i1);
		AX12SetFreeSpinSpeed(OmniID2, (short)i2);
		AX12SetFreeSpinSpeed(OmniID3, (short)i3);
		break;
			
	case DO_SIMPLE_POSE:
	
	
		UsingPoseSystem = 1;  //Stop using Dynamic Gait	
		for (int i=1; i <= 18; i++) {
		AX12SetPosition(i, (int)(pgm_read_word(&CustomPoses[i1*18+i-1])), 100);
		}
		
		break;
		
	case RELEASE_MOTORS: //Turn off the motors
		UsingPoseSystem = 1;  //Stop updating motors
		for (int i=1; i <= 18; i++) {
		AX12WriteUShort(i,AX12_TORQUE_ENABLE,0);  //Set Torque Enable off
		}
		CommsPrintf("Motors Off\r\n");
		break;
		
	case SERVO_TRIM:
		ServoTrim[i1] = i2;
		SendServoTargetPos(i1, ServoPos[i1] + ServoTrim[i1], 200);
		break;
		
	case BUGGY_MOVE:
	case BALANCE:
		break;
		
	case REPORT_IMU:
		if (i1 ==0){i1 = 0x78;}  //Use default value of 120 if none specified
		IMUGetRaw(i1,&Raw);
		CommsPrintf("%d %d %d %d %d %d\r\n",(int)Raw.AccelSide,(int)Raw.AccelFore,(int)Raw.AccelVert,(int)Raw.GyroPitch,
			(int)Raw.GyroRoll,(int)Raw.GyroYaw);
		break;
		
	case REPORT_IMU_CONT:
		if (i1 ==0){i1 = 0x78;}  //Use default value of 122 if none specified
		IMUGetRaw(i1,&Raw);
		IMU_ID = i1;
		CommsPrintf("%d %d %d %d %d %d\r\n",(int)Raw.AccelSide,(int)Raw.AccelFore,(int)Raw.AccelVert,(int)Raw.GyroPitch,
			(int)Raw.GyroRoll,(int)Raw.GyroYaw);
		if (IMU_Report_Flag==0){IMU_Report_Flag=1;}
		else {IMU_Report_Flag=0;}
		
		break;
	case REPORT_FOOT:
		if (i1 ==0){i1 = 0x78;}  //Use default value of 122 if none specified
		IMUGetRaw(i1,&Raw);
		
		CommsPrintf("%d %d %d %d\r\n",(int)Raw.AccelSide,(int)Raw.AccelFore,(int)Raw.AccelVert,(int)Raw.GyroPitch);
		break;
		
		
	case REPORT_FOOT_CONT:
		if (i1 ==0){i1 = 0x7A;}  //Use default value of 122 if none specified
		IMUGetRaw(i1,&Raw);
		Foot_ID = i1;
		CommsPrintf("%d %d %d %d\r\n",(int)Raw.AccelSide,(int)Raw.AccelFore,(int)Raw.AccelVert,(int)Raw.GyroPitch);
		if (Foot_Report_Flag==0){Foot_Report_Flag=1;}
		else {Foot_Report_Flag=0;}
		
		break;
		
	case WRITE_VAL:
		i3 = i3 & 0xFF;
		//CommsPrintf("%d %d %d \r\n",i1,i2,i3);
		RetVal = AX12WriteUShort(i1,i2,i3);
		//CommsPrintf("%d\r\n",(int)RetVal);
		AX12ReadMemory(i1, i2, 1, temp_char);
		CommsPrintf("%d \r\n",(int)temp_char[0]);
			//(int)temp_char[4],(int)temp_char[5],(int)temp_char[6],(int)temp_char[7],(int)temp_char[8],(int)temp_char[9]);
		break;
		
	case REPORT_SERVO:
		CommsPrintf("%d %d %d \r\n",i1,i2,i3);
		servo_report = 1; //Not implemented in this version
		if (i1%2 == 0){servo_report *=2;}
		if (i1%3 == 0){servo_report *=3;}
		if (i1%5 == 0){servo_report *=5;}
		CommsPrintf("%d\r\n",servo_report);
	
	case REPORT_ARMS:
		ArmReporting = i1;
		break;
		
	case L_ARM_POS:
		ServoPos[JID_L_SHDR_FORE+1]		= SERVO_NOT_CONNECTED;
		ServoPos[JID_L_SHDR_SPLAY+1]	= SERVO_NOT_CONNECTED;
		ServoPos[JID_L_ELBOW+1]			= SERVO_NOT_CONNECTED;
		ServoPos[JID_R_SHDR_FORE+1]		= SERVO_NOT_CONNECTED;
		ServoPos[JID_R_SHDR_SPLAY+1]	= SERVO_NOT_CONNECTED;
		ServoPos[JID_R_ELBOW+1]			= SERVO_NOT_CONNECTED;

		AX12SetPosition(JID_L_SHDR_FORE+1,	(unsigned short)i1, AX12_MAX_SPEED);
		AX12SetPosition(JID_L_SHDR_SPLAY+1,	(unsigned short)i2, AX12_MAX_SPEED);
		AX12SetPosition(JID_L_ELBOW+1,		(unsigned short)i3, AX12_MAX_SPEED);
		break;

	case READ_ALL_SERVOS:
		for (int i=1; i <= 18; i++) {
		AX12ReadMemory(i, AX12_PRESENT_POSITION_L, 2, temp_char);
		temp_one = (int)temp_char[0]+256*(int)temp_char[1];
		CommsPrintf("%d ",temp_one);
		}
		CommsPrintf("\r\n");
		break;

	case R_ARM_POS:
		ServoPos[JID_L_SHDR_FORE+1]		= SERVO_NOT_CONNECTED;
		ServoPos[JID_L_SHDR_SPLAY+1]	= SERVO_NOT_CONNECTED;
		ServoPos[JID_L_ELBOW+1]			= SERVO_NOT_CONNECTED;
		ServoPos[JID_R_SHDR_FORE+1]		= SERVO_NOT_CONNECTED;
		ServoPos[JID_R_SHDR_SPLAY+1]	= SERVO_NOT_CONNECTED;
		ServoPos[JID_R_ELBOW+1]			= SERVO_NOT_CONNECTED;

		AX12SetPosition(JID_R_SHDR_FORE+1,	(unsigned short)i1, AX12_MAX_SPEED);
		AX12SetPosition(JID_R_SHDR_SPLAY+1,	(unsigned short)i2, AX12_MAX_SPEED);
		AX12SetPosition(JID_R_ELBOW+1,		(unsigned short)i3, AX12_MAX_SPEED);
		break;

	case SERVO_POSITION:
		AX12SetPosition((unsigned char)i1, (unsigned short)i2, (unsigned short)i3);
		break;

	case SERVO_ROTATE:
	case TURN_SERVO:
		AX12SetFreeSpinSpeed((unsigned char)i1, (short)i2);
		break;
		
	case HIP_TRIM:
		ServoTrim[JID_R_HIP_FORE+1] = i1 * -1;
		ServoTrim[JID_L_HIP_FORE+1] = i1;

		if (ROBOTSTATUS != DO_PAGE)
		{
			SendServoTargetPos(JID_R_HIP_FORE+1, ServoPos[JID_R_HIP_FORE+1] + ServoTrim[JID_R_HIP_FORE+1], 200);
			SendServoTargetPos(JID_L_HIP_FORE+1, ServoPos[JID_L_HIP_FORE+1] + ServoTrim[JID_L_HIP_FORE+1], 200);
		}
		break;
		
	case HIGH_LEVEL_GAIT:
		
		//  Set the turn to the turn parameter  
		Turn = i2;
		//  Do the map through the parameter space based on the forward value
		//  If the forward value is small, start by ramping up the swing and stride height
		//
		if (i1 > forward_counter) {forward_counter += 1;}
		if (i1 < forward_counter) {forward_counter -= 1;}
		if (i1 = 0) {forward_counter = 0;}
		if (forward_counter < 5 && forward_counter > -5){
			Swing = abs(forward_counter/2);
			Stride_Height = abs(forward_counter * 7/4)+1;
			NumPoses = 28 - abs(forward_counter);
			Stride_Length = 0;}
		else{
			Camber = forward_counter + 5;
			//Tilt = raw_tilt + forward_counter;
			Stride_Length = forward_counter;
		}
		break;
		
		
	case SET_GAIT_PARAM:
		
		switch(i1)
		{
			case 0:
				Stride_Length = i2;
			break;
			
			case 1:
				Stride_Height = i2;
			break;
			
			case 2:
				Swing = i2;
			break;
			
			case 3:
				Stride_X_offset = i2;
			break;
			
			case 4:
				Stride_Y_offset = i2;
			break;
			
			case 5:
				Ticks_Per_Pose = i2;
			break;
			
			case 6:
				Tilt = i2;
			break;
			
			case 7:
				Turn = i2;
			break;
			
			case 8:
				NumPoses = i2;
			break;
			
			case 9:
				Camber = i2;
			break;
			
			case 10:
				SideStepTrip = i2;
			break;

		}
		
	break;
	}
}

void SendPacketS(char Command, char *Data) // Data must contain 12 characters.  NULL is optional
{
	char	Packet[PACKET_LEN + 1];
	MakePacket1(Packet, Command, Data);	
	TxDString((unsigned char *)Packet);
}

void SendPacketI(char Command, int i1, int i2, int i3)
{
	char	Packet[PACKET_LEN + 1];
	MakePacket2(Packet, Command, i1, i2, i3);	
	TxDString((unsigned char *)Packet);
}


/*
void OnGoodPacket()
{
	if(Packet[1]==DO_PAGE)
	{
		ROBOTSTATUS = DO_PAGE;
		doPage(Packet[2]);
		//TxDString("[Doing]");
	}
	
	if(Packet[1]==DO_POSE)
	{
		ROBOTSTATUS = DO_POSE;
		doPose(Packet[2],Packet[3]);
	}
	
	if(Packet[1]==STOP_POSE)
	{
		ROBOTSTATUS = STOP_POSE;
		TxDString("[NotImpl]");
	}
	
	if(Packet[1]==STOP_PAGE)
	{
		ROBOTSTATUS = STOP_PAGE;
		TxDString("[NotImpl]");
	}
	
	if(Packet[1]==BUGGY_MOVE)
	{
		buggy_move(Packet[2]);
	}
	
	if(Packet[1]==TURN_SERVO)
	{	
		uint16_t HBP = Packet[3];
		int16_t ThetaTarget = (HBP<<8) + Packet[4];
		uint16_t HBS = Packet[5];
		int16_t OmegaServo = (HBS<<8) + Packet[6];
		SendServoTargetPos(Packet[2],ThetaTarget,OmegaServo);
	}
	
	if(Packet[1]==SERVO_TRIM)
	{
		if(Packet[2] <= NUM_OF_SERVOS_ATTACHED){
			ServoTrim[Packet[2]]=UnsignedCharToChar(Packet[3]);
			char temp[30]; sprintf(temp,"\r\n[trim(%i)=%i ]\r\n",Packet[2],ServoTrim[Packet[2]]); TxDString(temp);
		}
	}
	
	if(Packet[1]==BALANCE)
	{
		if(Packet[2] == 0){
			Balance = BALANCE_OFF;
		}else{
			Balance = BALANCE_ON;
		}
	}
	
	if(Packet[1]==VERSION)
	{
		TxDString("[BLV1.11]");
	}
	
}
*/

/**************************************************************************\
*   Process the bad messages
*   (ignore but let user know by toggling the bad packet LED)
\**************************************************************************/

void OnErr(int ErrNo)
{
    //CommsSend("EH?",3,DBG_BLOCK);
    //Set_LED(LED_RED, LED_TOGGLE);                // Toggle the bad packet LED
}


/**************************************************************************\
*   Initialise the baudrate and enable UARTs and setup buffers
\**************************************************************************/

/**
CommsInitialize() set Serial Port to initial state.
Vide Mega128 Data sheet about Setting bit of register.
SerialInitialize() needs port, Baud rate, Interrupt value.

*/
void CommsInitialize(unsigned char bPort, unsigned char bBaudrate, unsigned char bInterrupt)
{
  TxBytes=0;
  TxPtr=TxBuff;
  RxPtr=RxBuff;
  if(bPort == SERIAL_PORT0)
  {
    UBRR0H = 0; UBRR0L = bBaudrate; 
    UCSR0A = 0x02;  UCSR0B = 0x18;
    if(bInterrupt&RX_INTERRUPT) sbi(UCSR0B,7); // RxD interrupt enable
    UCSR0C = 0x06; UDR0 = 0xFF;
    sbi(UCSR0A,6);//SET_TXD0_FINISH; // Note. set 1, then 0 is read
  }

  else if(bPort == SERIAL_PORT1)
  {
    UBRR1H = 0; UBRR1L = bBaudrate; 
	/*
    UCSR1A = 0x02;  UCSR1B = 0x18;
    if(bInterrupt&RX_INTERRUPT) sbi(UCSR1B,7); // RxD interrupt enable
    UCSR1C = 0x06; UDR1 = 0xFF;
    sbi(UCSR1A,6);//SET_TXD1_FINISH; // Note. set 1, then 0 is read
	*/
	UCSR1B = (1 << RXEN) | (1 << RXCIE) | (1 << TXCIE) | (1 << TXEN);
	UCSR1C = (1 << USBS) | (3 << UCSZ0);
	UDR1 = 0xFF;
  }
}

/**************************************************************************\ 
TxDString() prints data in ACSII code.
\**************************************************************************/
void TxDString(unsigned char *bData)
{
	//CommsSend(bData, strlen(bData), MRG_BLOCK);
	CommsSend(bData, strlen((char *)bData),FUL_BLOCK);
/*
  while(*bData)
  {
    TxD8(*bData++);
  }*/
}



/**************************************************************************\
*   Convert received characters into a speed value
\**************************************************************************/

int UnsignedCharToChar(unsigned char uchar)
{
    int RetVal = (int)(uchar & 0x7F);
    if (uchar & 0x80)
        RetVal *= -1;
    return RetVal;
}



